#pragma config(Sensor, in1,    lineFollowerRIGHT, sensorLineFollower)
#pragma config(Sensor, in2,    lineFollowerCENTER, sensorLineFollower)
#pragma config(Sensor, in3,    lineFollowerLEFT, sensorLineFollower)
#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Motor,  port1,           rightMotor,    tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port10,          leftMotor,     tmotorVex393, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*----------------------------------------------------------------------------------------------------*\
|*                             	    - Triple Sensor Line Tracking -                                   *|
|*                                      ROBOTC on VEX 2.0 CORTEX                                      *|
|*                                                                                                    *|
|*  This program uses 3 VEX Line Follower Sensors to track a black line on a light(er) surface.       *|
|*  There is a two second pause at the beginning of the program.                                      *|
|*                                                                                                    *|
|*                                        ROBOT CONFIGURATION                                         *|
|*    NOTES:                                                                                          *|
|*    1)  Reversing 'rightMotor' (port 2) in the "Motors and Sensors Setup" is needed with the        *|
|*        "Squarebot" mode, but may not be needed for all robot configurations.                       *|
|*    2)  Lighting conditions, line darkness, and surface lightness change from place to place,       *|
|*        so the value of 'threshold' may need to be changed to better suit your environment.         *|
|*                                                                                                    *|
|*    MOTORS & SENSORS:                                                                               *|
|*    [I/O Port]          [Name]              [Type]                [Description]                     *|
|*    Motor  - Port 2     rightMotor          VEX 3-wire module     Right side motor                  *|
|*    Motor  - Port 3     leftMotor           VEX 3-wire module     Left side motor                   *|
|*    Analog - Port 1     lineFollowerRIGHT   VEX Light Sensor      Front-right, facing down          *|
|*    Analog - Port 2     lineFollowerCENTER  VEX Light Sensor      Front-center, facing down         *|
|*    Analog - Port 3     lineFollowerLEFT    VEX Light Sensor      Front-left, facing down           *|
\*-----------------------------------------------------------------------------------------------4246-*/


//+++++++++++++++++++++++++++++++++++++++++++++| MAIN |+++++++++++++++++++++++++++++++++++++++++++++++
int x,y;
task main()
{
	wait1Msec(2000);          // The program waits for 2000 milliseconds before continuing.

	int threshold = 2600;      /* found by taking a reading on both DARK and LIGHT    */
	/* surfaces, adding them together, then dividing by 2. */
	while (SensorValue[lineFollowerCENTER] <= threshold || SensorValue[lineFollowerRIGHT] <= threshold)
	{

		// RIGHT sensor sees dark:
		if(SensorValue(lineFollowerRIGHT) > threshold)
		{
			// counter-steer right:

			motor[leftMotor]  = 63;
			motor[rightMotor] = 30;
		}
		// CENTER sensor sees dark:
		if(SensorValue(lineFollowerCENTER) > threshold)
		{
			// go straight
			motor[leftMotor]  = 63;
			motor[rightMotor] = 63;
		}
		// LEFT sensor sees dark:
		if(SensorValue(lineFollowerLEFT) > threshold)
		{
			// counter-steer left:
			motor[leftMotor]  = 30;
			motor[rightMotor] = 63;
		}
	}

	motor[leftMotor] = 63;
	motor[rightMotor] = 63;
	wait1Msec(500);
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);
	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;

	while (SensorValue[rightEncoder] >= -382 || SensorValue[leftEncoder] <= 382)
	{
		x=SensorValue[leftEncoder];
		y=SensorValue[rightEncoder];
		motor[leftMotor] = 80;
		motor[rightMotor] = -80;
		if(SensorValue[rightEncoder] <= -382) {
			motor[rightMotor] = 0;
		}
		if(SensorValue[leftEncoder] >= 382)
		{
			motor[leftMotor] = 0;
		}
	}
	while (SensorValue[lineFollowerCENTER] <= threshold || SensorValue[lineFollowerRIGHT] <= threshold)
	{

		// RIGHT sensor sees dark:
		if(SensorValue(lineFollowerRIGHT) > threshold)
		{
			// counter-steer right:

			motor[leftMotor]  = 63;
			motor[rightMotor] = 30;
		}
		// CENTER sensor sees dark:
		if(SensorValue(lineFollowerCENTER) > threshold)
		{
			// go straight
			motor[leftMotor]  = 63;
			motor[rightMotor] = 63;
		}
		// LEFT sensor sees dark:
		if(SensorValue(lineFollowerLEFT) > threshold)
		{
			// counter-steer left:
			motor[leftMotor]  = 30;
			motor[rightMotor] = 63;
		}
	}

	motor[leftMotor] = 63;
	motor[rightMotor] = 63;
	wait1Msec(500);
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);
	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;

	while (SensorValue[rightEncoder] >= -382 || SensorValue[leftEncoder] <= 382)
	{
		x=SensorValue[leftEncoder];
		y=SensorValue[rightEncoder];
		motor[leftMotor] = 80;
		motor[rightMotor] = -80;
		if(SensorValue[rightEncoder] <= -382) {
			motor[rightMotor] = 0;
		}
		if(SensorValue[leftEncoder] >= 382)
		{
			motor[leftMotor] = 0;
		}
	}
	wait1Msec(3000);
	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;

	while (SensorValue[rightEncoder] >= -382 || SensorValue[leftEncoder] <= 382)
	{
		x=SensorValue[leftEncoder];
		y=SensorValue[rightEncoder];
		motor[leftMotor] = 80;
		motor[rightMotor] = -80;
		if(SensorValue[rightEncoder] <= -382) {
			motor[rightMotor] = 0;
		}
		if(SensorValue[leftEncoder] >= 382)
		{
			motor[leftMotor] = 0;
		}
	}
	while (SensorValue[lineFollowerCENTER] <= threshold || SensorValue[lineFollowerRIGHT] <= threshold)
	{

		// RIGHT sensor sees dark:
		if(SensorValue(lineFollowerRIGHT) > threshold)
		{
			// counter-steer right:

			motor[leftMotor]  = 63;
			motor[rightMotor] = 30;
		}
		// CENTER sensor sees dark:
		if(SensorValue(lineFollowerCENTER) > threshold)
		{
			// go straight
			motor[leftMotor]  = 63;
			motor[rightMotor] = 63;
		}
		// LEFT sensor sees dark:
		if(SensorValue(lineFollowerLEFT) > threshold)
		{
			// counter-steer left:
			motor[leftMotor]  = 30;
			motor[rightMotor] = 63;
		}
	}

	motor[leftMotor] = 63;
	motor[rightMotor] = 63;
	wait1Msec(500);
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);
	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;

	while (SensorValue[rightEncoder] >= 382 || SensorValue[leftEncoder] <= -382)
	{
		x=SensorValue[leftEncoder];
		y=SensorValue[rightEncoder];
		motor[leftMotor] = -80;
		motor[rightMotor] = 80;
		if(SensorValue[rightEncoder] <= 382) {
			motor[rightMotor] = 0;
		}
		if(SensorValue[leftEncoder] >= -382)
		{
			motor[leftMotor] = 0;
		}
	}
	while (SensorValue[lineFollowerCENTER] <= threshold || SensorValue[lineFollowerRIGHT] <= threshold)
	{

		// RIGHT sensor sees dark:
		if(SensorValue(lineFollowerRIGHT) > threshold)
		{
			// counter-steer right:

			motor[leftMotor]  = 63;
			motor[rightMotor] = 30;
		}
		// CENTER sensor sees dark:
		if(SensorValue(lineFollowerCENTER) > threshold)
		{
			// go straight
			motor[leftMotor]  = 63;
			motor[rightMotor] = 63;
		}
		// LEFT sensor sees dark:
		if(SensorValue(lineFollowerLEFT) > threshold)
		{
			// counter-steer left:
			motor[leftMotor]  = 30;
			motor[rightMotor] = 63;
		}
	}

	motor[leftMotor] = 63;
	motor[rightMotor] = 63;
	wait1Msec(500);
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);


}

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
